﻿#include <gtest/gtest.h>

#include "algo/algo_interpolate.h"
#include "algo/algo_projection.h"
#include "geom/geom_curve_bezier.h"
#include "geom/geom_curve_bspline.h"
#include "geom/geom_surface_bspline.h"
#include "kernel/debug_timer.h"
#include "kernel/spline.h"

#include <algorithm>
#include <iostream>
#include <random>
#include <time.h>

#include <dbg.h>

using namespace CAGD;

class TestProjection : public ::testing::Test
{
  public:
    // 初始化函数，其中初始化要测试的数据
    void SetUp() override
    {
        generator = std::mt19937((unsigned int)time(nullptr)); // 使用 time(nullptr) 作为随机数生成器的种子
        uniform = std::uniform_real_distribution<double>(0, 10.0); // 在 [0,1] 区间内的均匀分布

        // 初始化曲线
        {
            int N = 100;
            std::vector<BPnt> points;
            for (int i = 0; i < N; i++)
                points.push_back({uniform(generator), uniform(generator), uniform(generator)});
            curve = Interpolate::BSplineCurve3(points);
        }

        // 初始化曲面
        {
            // int N = 100;
            // int M = 150;
            // std::vector<std::vector<BPnt>> points(N, std::vector<BPnt>(M));
            // for (int i = 0; i < N; i++)
            //     for (int j = 0; j < M; j++)
            //         points[i][j] = {uniform(generator), uniform(generator), uniform(generator)};
            // surface = Interpolate::BSplineSurface3x3(points);

            auto f = [](double u, double v) -> BPnt { return BPnt{u, v, sin(u + v) * cos(u - v)}; };
            // 20 x 30 个采样点
            int N = 10;
            int M = 15;
            std::vector<std::vector<BPnt>> points(N, std::vector<BPnt>(M));
            const double PI = 3.1415926535;
            for (int i = 0; i < N; i++)
                for (int j = 0; j < M; j++)
                    points[i][j] = f(10.0 / (N - 1) * i, 10.0 / (M - 1) * j);
            surface = Interpolate::BSplineSurface3x3(points);
        }
    }

    // 析构函数，用来销毁测试数据，这里不需要
    // void TearDown() override {}

    Handle(GeomBSplineCurve) curve;
    Handle(GeomBSplineSurface) surface;

    std::mt19937 generator;
    std::uniform_real_distribution<double> uniform;
};

// 测试点到曲线的投影函数
TEST_F(TestProjection, Point2Curve)
{
    int M = 1000;
    int valid = 0;
    for (int i = 0; i < M; i++)
    {
        BPnt P = {uniform(generator), uniform(generator), uniform(generator)};
        double t = 0.5;
        BPnt Q;
        BVec V1;

        // 测试最近投影（不稳定，需要接近最近点）
        // auto distance = Projection::Near(curve, P, t, Q, CAGD_ABS_ACC);
        // curve->D1(t, Q, V1);
        // double angle = V1.Normalized() * (P - Q).Normalized();
        // EXPECT_TRUE(distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(t) < CAGD_ABS_ACC * 2) ||
        //             (fabs(1 - t) < CAGD_ABS_ACC * 2));

        // 测试 1 阶正交投影（不稳定，需要接近最近点）
        // distance = Projection::Ortho1(curve, P, t, Q, CAGD_ABS_ACC);
        // curve->D1(t, Q, V1);
        // angle = V1.Normalized() * (P - Q).Normalized();

        // EXPECT_TRUE(distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(t) < CAGD_ABS_ACC * 2) ||
        //             (fabs(1 - t) < CAGD_ABS_ACC * 2));

        // 测试 2 阶正交投影（较稳定）
        auto distance = Projection::Ortho2(curve, P, t, Q, CAGD_ABS_ACC);
        curve->D1(t, Q, V1);
        auto angle = V1.Normalized() * (P - Q).Normalized();

        // EXPECT_TRUE(distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(t) < CAGD_ABS_ACC * 2) ||
        //             (fabs(1 - t) < CAGD_ABS_ACC * 2));

        if (distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC)
            valid++;
    }

    // dbg(1.0 * valid / M);
    EXPECT_TRUE(1.0 * valid / M > 0.95);
}

// 测试点到曲面的投影函数
TEST_F(TestProjection, Point2Surface)
{
    double U1, U2, V1, V2;
    surface->Bounds(U1, U2, V1, V2);

    TIMER_START(Near)

    int T = 1000;
    int valid = 0;
    for (int i = 0; i < T; i++)
    {
        BPnt P = {uniform(generator), uniform(generator), uniform(generator)};
        double u = 0.5, v = 0.5;
        BPnt Q;

        auto distance = Projection::Near(surface, P, u, v, Q, CAGD_ABS_ACC);
        BVec V = surface->Normal(u, v);
        double angle = (V.Normalized() % (P - Q).Normalized()).Modulus();

        if (distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(u - U1) < CAGD_ABS_ACC * 2) ||
            (fabs(u - U2) < CAGD_ABS_ACC * 2) || (fabs(v - V1) < CAGD_ABS_ACC * 2) || (fabs(v - V2) < CAGD_ABS_ACC * 2))
            valid++;
    }

    TIMER_END(Near)

    // dbg(1.0 * valid / T);
    EXPECT_TRUE(1.0 * valid / T > 0.7);
    valid = 0;

    TIMER_START(Ortho1)

    for (int i = 0; i < T; i++)
    {
        BPnt P = {uniform(generator), uniform(generator), uniform(generator)};
        double u = 0.5, v = 0.5;
        BPnt Q;

        double angle = Projection::Ortho1(surface, P, u, v, Q, CAGD_ABS_ACC);
        auto distance = P.Distance(Q);

        if (distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(u - U1) < CAGD_ABS_ACC * 2) ||
            (fabs(u - U2) < CAGD_ABS_ACC * 2) || (fabs(v - V1) < CAGD_ABS_ACC * 2) || (fabs(v - V2) < CAGD_ABS_ACC * 2))
            valid++;
    }

    TIMER_END(Ortho1)

    // dbg(1.0 * valid / T);
    EXPECT_TRUE(1.0 * valid / T > 0.5);
    valid = 0;

    TIMER_START(Ortho2)

    for (int i = 0; i < T; i++)
    {
        BPnt P = {uniform(generator), uniform(generator), uniform(generator)};
        double u = 0.5, v = 0.5;
        BPnt Q;

        double angle = Projection::Ortho2(surface, P, u, v, Q, CAGD_ABS_ACC);
        auto distance = P.Distance(Q);

        if (distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(u - U1) < CAGD_ABS_ACC * 2) ||
            (fabs(u - U2) < CAGD_ABS_ACC * 2) || (fabs(v - V1) < CAGD_ABS_ACC * 2) || (fabs(v - V2) < CAGD_ABS_ACC * 2))
            valid++;
    }

    TIMER_END(Ortho2)

    // dbg(1.0 * valid / T);
    EXPECT_TRUE(1.0 * valid / T > 0.8);
    valid = 0;

    TIMER_START(Torus)

    for (int i = 0; i < T; i++)
    {
        BPnt P = {uniform(generator), uniform(generator), uniform(generator)};
        double u = 0.5, v = 0.5;
        BPnt Q;

        double angle = Projection::Torus(surface, P, u, v, Q, CAGD_ABS_ACC);
        auto distance = P.Distance(Q);

        if (distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(u - U1) < CAGD_ABS_ACC * 2) ||
            (fabs(u - U2) < CAGD_ABS_ACC * 2) || (fabs(v - V1) < CAGD_ABS_ACC * 2) || (fabs(v - V2) < CAGD_ABS_ACC * 2))
            valid++;
    }

    TIMER_END(Torus)

    // dbg(1.0 * valid / T);
    EXPECT_TRUE(1.0 * valid / T > 0.9);
    valid = 0;
}

#include "math/linear_math.h"

// 测试曲线到曲面的投影函数
TEST_F(TestProjection, Curve2Surface)
{
    double U1, U2, V1, V2;
    surface->Bounds(U1, U2, V1, V2);

    int valid = 0;
    int T = 1000;

    TIMER_START(March)

    std::vector<BPnt2d> para;
    Projection::March(curve, surface, 0, 1, T, CAGD_ABS_ACC, para);
    double k = 1.0 / T;

    TIMER_END(March)

    for (int i = 0; i <= T; i++)
    {
        double t = k * i;
        auto P = curve->Value(t);

        BPnt Q;
        BVec DU, DV;
        surface->D1(para[i][0], para[i][1], Q, DU, DV);
        auto N = (DU % DV).Normalized();

        double angle = (N % (P - Q)).Modulus();
        auto distance = P.Distance(Q);

        auto u = para[i][0];
        auto v = para[i][1];
        if (distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(u - U1) < CAGD_ABS_ACC * 2) ||
            (fabs(u - U2) < CAGD_ABS_ACC * 2) || (fabs(v - V1) < CAGD_ABS_ACC * 2) || (fabs(v - V2) < CAGD_ABS_ACC * 2))
            valid++;
    }

    // dbg(1.0 * valid / T);
    EXPECT_TRUE(1.0 * valid / T > 0.5);
    valid = 0;
    para.clear();

    TIMER_START(ODE)

    Projection::ODE(curve, surface, 0, 1, T, CAGD_ABS_ACC, para);

    TIMER_END(ODE)

    for (int i = 0; i <= T; i++)
    {
        double t = k * i;
        auto P = curve->Value(t);

        BPnt Q;
        BVec DU, DV;
        surface->D1(para[i][0], para[i][1], Q, DU, DV);
        auto N = (DU % DV).Normalized();

        double angle = (N % (P - Q)).Modulus();
        auto distance = P.Distance(Q);

        auto u = para[i][0];
        auto v = para[i][1];
        if (distance < CAGD_ABS_ACC || fabs(angle) < CAGD_ABS_ACC || (fabs(u - U1) < CAGD_ABS_ACC * 2) ||
            (fabs(u - U2) < CAGD_ABS_ACC * 2) || (fabs(v - V1) < CAGD_ABS_ACC * 2) || (fabs(v - V2) < CAGD_ABS_ACC * 2))
            valid++;
    }

    // dbg(1.0 * valid / T);
    EXPECT_TRUE(1.0 * valid / T > 0.7);
    valid = 0;
}